#include "haptics.h"
CHaptics::CHaptics()
{
	// Let's reset all the values we need to a default value (99% null)
	for(int i = 0; i < 3; i++)
	{
		m_posServo[i]	= 0;
		m_forceServo[i]	= 0;
		m_buttonServo	= 0;
		
		m_posApp[i]	= 0;
		m_buttonApp = 0;
	}
}

CHaptics::~CHaptics()
{
	// Destroy it!
	// All right, let's UnInit. This has to be in reverse order of Init()
	if(ServoOp != HDL_INVALID_HANDLE)
	{
		hdlDestroyServoOp(ServoOp);
		ServoOp = HDL_INVALID_HANDLE;
	}
	
	hdlStop();
	
	if(devHandle != HDL_INVALID_HANDLE)
	{
		hdlUninitDevice(devHandle);
		devHandle = HDL_INVALID_HANDLE;
	}
}

// The main callback function, the one with the update-frequency of 1kHz
HDLServoOpExitCode ContactCB(void* pParam)
{
	// Get pointer to haptic class
	CHaptics* Device = static_cast< CHaptics* >( pParam );

	// Get current state of haptic device
    hdlToolPosition(Device->m_posServo);
    hdlToolButton(&(Device->m_buttonServo));

	// Our junk function, to test stuff
	Device->Test();

	// Set forces to haptic device
	hdlSetToolForce(Device->m_forceServo);

	return HDL_SERVOOP_CONTINUE;
}

// The on-demand callback function, for safe reading
HDLServoOpExitCode GetStateCB(void* pParam)
{
	// Get pointer to haptic class
	CHaptics* Device = static_cast< CHaptics* >( pParam );

	// Call synchronizing function, to safely read the data
	Device->synch();

	return HDL_SERVOOP_EXIT;
}

int CHaptics::Init()
{
	// Now lets init the device. DEFAULT equals 0, equals the default plugged Falcon in. See HDAL.ini for details
	// We assign it to Handle, so we use interact with it later
	devHandle = hdlInitNamedDevice("DEFAULT");
	
	// Let's test to see if it all went correctly
	if(!testHDLError("hdlInitDevice"))
		return 2;

	// Now let's see if the handle is created at all, if not, failed to open device
	if(devHandle == HDL_INVALID_HANDLE)
		return 3;

	// So everything went alright... right? Let's open it up, making it functional!
	hdlStart();
	if(!testHDLError("hdlStart"))
		return 4;

	// Let's create the Servo thread, to communicate haptic and application
	ServoOp = hdlCreateServoOp(ContactCB, this, bNonBlocking);
	if(!testHDLError("hdlCreateServoOp"))
		return 5;

	// Make the device current.  All subsequent calls will
	// be directed towards the current device
    hdlMakeCurrent(devHandle);
    if(!testHDLError("hdlMakeCurrent"))
		return 6;

	// Workspace: Returned dimensions in the array are minx, miny, minz, maxx, maxy, maxz
	//													left, bottom, far, right, top, near)
	hdlDeviceWorkspace(m_workspace);
	if(!testHDLError("hdlDeviceWorkspace"))
		return 7;

	// Check if the device is calibrated, if not, ask the user to do so
	if(!isDeviceCalibrated())
		return -1;

	return 1;
}

// This is a simple function for testing error returns.  A production
// application would need to be more sophisticated than this.
// Returns 0 if there is an error
bool CHaptics::testHDLError(std::string str)
{
    HDLError err = hdlGetError();
    if (err != HDL_NO_ERROR)
		return 0;

	return 1;
}

// Recreate the servo with a blocking type, to safely read the data
void CHaptics::synchFromServo()
{
	hdlCreateServoOp(GetStateCB, this, bBlocking);
}

// Synch data (mostly readin) between the app and device
void CHaptics::synch()
{
	m_buttonApp = m_buttonServo;
	for(int i = 0; i < 3; i++)
		m_posApp[i] = m_posServo[i];
}

// Synch and return position
void CHaptics::getPosition(double pos[3])
{
	synchFromServo();
	for(int i = 0; i < 3; i++)
		pos[i] = m_posApp[i];
}

// For this application, the only device status of interest is the
// calibration status.  A different application may want to test for
// HDAL_UNINITIALIZED and/or HDAL_SERVO_NOT_STARTED
bool CHaptics::isDeviceCalibrated()
{
    unsigned int state = hdlGetState();

    return ((state & HDAL_NOT_CALIBRATED) == 0);
}

bool CHaptics::isButtonDown()
{
	synchFromServo();
	return m_buttonApp;
}

void CHaptics::Test()
{
}